#include "fusion_perception.h"
#include "association/associate_factor.h"
#include "association/associator.h"
#include "global.h"
#include "time_sync/time_sync.h"
#include "tracker/tracker_manager.h"
#include "utils/utils.h"

#include <muduo/net/EventLoop.h>
#include <muduo/net/EventLoopThread.h>
#include <muduo/net/TimerId.h>

#include <chrono>
#include <iostream>

namespace fusion_perception {

FusionPerception::FusionPerception(void)
{
    auto& global = Global::getInstance();
    timeSyncSptr_ = std::make_shared<TimeSync>();

    trackerManagerSptr_ = std::make_shared<TrackerManager>();

    std::vector<std::shared_ptr<AssociateFactor>> associateFactors {
        std::make_shared<PositionXAssociateFactor>("x", 60),
        std::make_shared<PositionYAssociateFactor>("y", 60),
        //std::make_shared<HistoryIdAssociateFactor>("h", 300),
        //std::make_shared<TypeAssociateFactor>("t", 100),
        //std::make_shared<FeatureAssociateFactor>("f", 100),
    };

    associatorSptr_ = std::make_shared<Associator>(trackerManagerSptr_, associateFactors);

    eventLoopThreadSptr_ = std::make_shared<muduo::net::EventLoopThread>();
    timerIdSptr_ = std::make_shared<muduo::net::TimerId>(eventLoopThreadSptr_->startLoop()->runEvery(1.0 / global.hz, std::bind(&FusionPerception::onTimeout, this)));
}

FusionPerception::~FusionPerception(void)
{
    eventLoopThreadSptr_->startLoop()->cancel(*timerIdSptr_);
}

void FusionPerception::registerFusedResultCB(FusedResultCB fusedResultCB)
{
    slots_.emplace_back(signal_.connect(std::move(fusedResultCB)));
}

void FusionPerception::pushObjectSptrVector(std::string frameId, ObjectSptrVector objectSptrVector)
{
    timeSyncSptr_->push(std::move(frameId), std::move(objectSptrVector));
}

void FusionPerception::onTimeout(void)
{
    auto microTs = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::system_clock::now().time_since_epoch()).count();

    auto timeSyncObjectsSptr = timeSyncSptr_->take(microTs);

    std::cout << std::endl;
    for (auto& i : timeSyncObjectsSptr) {
        std::cout << i.first << ":" << i.second.size() << std::endl;
        for (auto& j : i.second) {
            std::cout << printObject(*j) << std::endl;
        }
    }

    auto LastFusedObjects = trackerManagerSptr_->getFusedObjects();

    auto multiAssociatedObjectSptr = associatorSptr_->associate(microTs, LastFusedObjects, timeSyncObjectsSptr);

    trackerManagerSptr_->fuse(microTs, multiAssociatedObjectSptr);

    auto fusedObjects = trackerManagerSptr_->getFusedObjects();

    std::cout << "output:" << fusedObjects.size() << std::endl;
    for (auto& i : fusedObjects) {
        std::cout << printObject(*i) << std::endl;
    }

    signal_.call(std::move(fusedObjects));
}

}